/*
 * Position2d.java
 *
 * Created on 15 de noviembre de 2007, 04:19 PM
 *
 */

package Robot;

import Robot.Sensors.Pose2DSensor;
import Robot.Sensors.Sensor;
import Utils.*;


/**
 * Base class for all Position2d interfaces. It implements position control
 * and orientation control (P Controller) (see method goToPos).
 * @author Alejandro Pustowka
 */
public abstract class Position2d extends Sensor implements Pose2DSensor {

    //boolean ENABLE_NOISE=false;


    //static final float RANDOM_LINEAR_VEL = 0.3f;
    float headingBias = 0.5f;

    //FIXME: EStos parametros deberian ser static para clases que heredan de esta.
    //Por ejemplo, hacer una clase que sea exclusiva para el nxbot, que ya tenga
    //estos parametros por defecto, a partir de las configuraciones realizadas
    /**
     * Angle limit to define whether the robot should only rotate to reach desired
     * heading.
     */
    float maxAngle = (float) Math.PI/10;

    /**
     * Angle limit to define wheter the robot should also rotate and go forward.
     */
    float corrAngle = (float) Math.PI/8;

    /**
     * Maximum speed of the robot.
     */
    protected float maxSpeed = 1.0f;

    /**
     * Maximum rotation velocity for orientation control
     */
    private float maxRotVel = 0.3f;
    /**
     * Mininum rotation velocity for orientation control
     */
    private float minRotVel = 0.3f;

    /**
     * Data returned from method read.
     */
    protected Pose2d data;
    
    
    /**
     * Creates a new instance of Position2d
     * @param robot The PlayerClient.
     * @param name Name of this interface.
     * @param index Index of this interface (see Player).
     */
    public Position2d(Robot robot, String name, int index) {
        //constructor of father-class
        super(robot, name, index);
    }

    /**
     * Read data from position2d interface. It returns position and orientation
     * of the interface.
     * @return The pose of the interface (x, y, theta)
     */
    public Pose2d read(int mode) {

        //if READ_MODE_WAIT it will waits until new data is available
        //else, returns the old data
        switch(mode)
        {
            case READ_MODE_WAIT:
                while(!update());
            break;
            case READ_MODE_NO_WAIT:
                update();
            break;
        
        }
        return data;   

    }

    /**
     * Returns the position of the interface. Heading is not taken into account.
     * If heading is also of interest, you should try read.
     * @return
     */
    public Vector2d getPosition(){
        return new Vector2d(data.getPx(),data.getPy(),Vector2d.MODE_XY);
    
    }

    //FIXME: esto debería recibir una pose para poder realizar control de orientacion
    //public boolean goToPos(Pose2d pose)
    //{ Vector2d posVector = new Vector2d(pose.getX(), pose.getY());

    //TODO: Falta implemmentar que retorne true o false.
    /**
     * Position control for the interface. This type of control uses another
     * interface to get the actual position, as when using NXVISION to get positions
     * of robots.
     * @param desiredPos Desired position vector.
     * @param pos Position2d from where to read the actual position.
     * @return True or false depending if the robot reached the goal or not.
     */
    public boolean goToPos(Vector2d desiredPos, Position2d pos){


        float corr;

        float magnitude = (float)desiredPos.getMag();
        float angle = (float)desiredPos.getAngle();

        //first check if the robot really has to move
        if(magnitude!=0)
        {
            //First, magnitude should not be bigger than 1
            if(magnitude>1)
                    magnitude=1;

            //update position values
            Pose2d actualPose = pos.read(READ_MODE_NO_WAIT);

            //calculate the headingDifference between the robots heading and the desired heading
            float headingDiff = angle - (float)actualPose.getPa();
            //Correction of angle, choose the shortest spin
            headingDiff = (float)Vector2d.angleCorrMPI2PI(headingDiff);

            if(debugMode == DEBUG_HIGH)
            {
                System.out.println("POSITION2D: Heading calculation");
                System.out.println("Heading of robot:" + Math.toDegrees(actualPose.getPa()));
                System.out.println("Desired heading:"+Math.toDegrees(angle));
                System.out.println("Heading difference:"+Math.toDegrees(headingDiff)+"\n");
            }
            //if the difference is bigger than the allowed then correct heading
            if(Math.abs(headingDiff)>maxAngle){

                //an aid to linear velocity. If the angle is very thin, then
                //add some linear velocity for soft drive.
                if(Math.abs(headingDiff) > corrAngle)
                    corr = 0;
                else
                    //this is the factor of linear velocity that will be added.
                    //it should not be bigger than 0.5. If the difference between
                    corr = 0.5f * (float)((Math.abs(headingDiff)-maxAngle) / (corrAngle-maxAngle));


                float corrSpeed = Math.abs(corr*magnitude);

                //Control proporcional de orientacion, donde el error es headingDiff
                //y la constante de proporcionalidad es minRotVel/maxAngle


                float rotVel = (float) (minRotVel*Math.abs(headingDiff)/maxAngle);

                if (rotVel >maxRotVel)
                    rotVel = maxRotVel;

                if(headingDiff<0)
                    setSpeed(corrSpeed,-rotVel);
                else
                    setSpeed(corrSpeed,rotVel);

                return false;

            }
            //the heading is right, then go forward
           else{

                //correction factor for angular velocity
                corr = headingDiff / maxAngle;
                corr *= headingBias;

                setSpeed(magnitude*maxSpeed,corr*magnitude);

           }
            return false;
        }
        else{
            setSpeed(0,0);

        }
        return false;
    }

    /**
     * Position control for the interface.
     * @param desiredPos Desired position vector.
     * @return True or false depending if the robot reached the goal or not.
     */
    public boolean goToPos(Vector2d desiredPos){


        float corr;
        
        float magnitude = (float)desiredPos.getMag();
        float angle = (float)desiredPos.getAngle();

        //first check if the robot really has to move
        if(magnitude!=0)
        {
            //First, magnitude should not be bigger than 1
            if(magnitude>1)
                    magnitude=1;

            //update position values
            Pose2d actualPose = read(READ_MODE_NO_WAIT);
            
            //calculate the headingDifference between the robots heading and the desired heading
            float headingDiff = angle - (float)actualPose.getPa();
            //Correction of angle, choose the shortest spin
            headingDiff = (float)Vector2d.angleCorrMPI2PI(headingDiff);

            if(debugMode == DEBUG_HIGH)
            {
                System.out.println("POSITION2D: Heading calculation");
                System.out.println("Heading of robot:" + Math.toDegrees(actualPose.getPa()));
                System.out.println("Desired heading:"+Math.toDegrees(angle));
                System.out.println("Heading difference:"+Math.toDegrees(headingDiff)+"\n");
            }
            //if the difference is bigger than the allowed then correct heading
            if(Math.abs(headingDiff)>maxAngle){
               
                //an aid to linear velocity. If the angle is very thin, then 
                //add some linear velocity for soft drive.
                if(Math.abs(headingDiff) > corrAngle)
                    corr = 0;
                else
                    //this is the factor of linear velocity that will be added.
                    //it should not be bigger than 0.5. If the difference between
                    corr = 0.5f * (float)((Math.abs(headingDiff)-maxAngle) / (corrAngle-maxAngle));

                
                float corrSpeed = Math.abs(corr*magnitude);

                //Control proporcional de orientacion, donde el error es headingDiff
                //y la constante de proporcionalidad es minRotVel/maxAngle


                float rotVel = (float) (minRotVel*Math.abs(headingDiff)/maxAngle);

                if (rotVel >maxRotVel)
                    rotVel = maxRotVel;

                if(headingDiff<0)
                    setSpeed(corrSpeed,-rotVel);
                else
                    setSpeed(corrSpeed,rotVel);
                
                return false;

            }
            //the heading is right, then go forward
           else{
                
                //correction factor for angular velocity
                corr = headingDiff / maxAngle;
                corr *= headingBias;

                setSpeed(magnitude*maxSpeed,corr*magnitude);
               
           }
            return false; 
        }
        else{
            setSpeed(0,0);
            
        }
        return false;
    }

    /**
     * Sets the maximum linear speed of this interface.
     * @param maxSpeed Max. speed of the interface.
     */
    public void setMaxSpeed(float maxSpeed)
    {
        this.maxSpeed = maxSpeed;
    }

    /**
     * Defines the limits of the P Controller of the orientation. When the
     * difference between the desired orientation and the actual orientation is
     * maximum (that means, difference equals PI), the interface rotates using the
     * velocity defined in maxRotVel. When it is reaching the desired heading,
     * the velocity decreases linearly until it reaches the minRotVel.
     *
     * @param minRotVel Min. vel allowed to the rotation.
     * @param maxRotVel Max. vel  allowed to the rotation.
     */
    public void setRotControl(float minRotVel, float maxRotVel)
    {
        this.maxRotVel = maxRotVel;
        this.minRotVel = minRotVel;

    }

    /**
     * Defines the limits that defines the behavior of the position controller.
     * Check maxAngle and corrAngle variables.
     * @param maxAngle The new maxAngle value.
     * @param corrAngle The new corrAngle value.
     */
    public void setAngControl(float maxAngle, float corrAngle)
    {
        this.maxAngle = maxAngle;
        this.corrAngle = corrAngle;
    }
    
    
    
    public abstract void setSpeed(float val1, float val2);
    public abstract boolean update();
    public abstract boolean isReady();
    public abstract void setPosition(Pose2d initial);
    
    
}
